using System;
using System.Collections.Generic;
using System.Collections.Specialized;
using System.ComponentModel;
using System.Net;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.Core.DsspHttpUtilities;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using submgr = Microsoft.Dss.Services.SubscriptionManager;


using vex = VEXRobotics.Proxy;
using robotics = Microsoft.Robotics;
using battery = Microsoft.Robotics.Services.Battery.Proxy;

///
/// This is a simple battery monitor for the robot.  The design is
/// a 4.3 volt zener diode connected to the battery to a 4.7k resistor
/// to ground.  This voltage is then fed to an analog input for
/// monitoring.  Simple, but it works!
/// 
namespace VEXExtras.VEXBattery
{
    /// <summary>
    /// Get the VEX Battery State
    /// </summary>
    [Description("Gets the current state of the VEX Battery.")]
    public class Get : Get<GetRequestType, PortSet<VEXBatteryState, Fault>>
    {
    }

    [ServicePort]
    public class VEXBatteryOperations : PortSet<
        DsspDefaultLookup,
        DsspDefaultDrop,
        Get,
        HttpGet,
        HttpPost,
        battery.Replace,
        battery.Subscribe,
        battery.SetCriticalLevel>
    {
    }

    /// <summary>
    /// VEX Battery Service
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract(battery.Contract.Identifier)]
    [DssServiceDescription("http://msdn.microsoft.com/library/bb870566.aspx")]
    [DisplayName("VEX Battery Control")]
    [Description("Provides access to the Battery.")]
    [DssCategory(vex.VEXCategories.VEX)]
    public class VEXBattery : DsspServiceBase
    {
        /// <summary>
        /// XSLT Transform to use for display
        /// </summary>
        [EmbeddedResource("Robotics.VEXExtras.VEXBattery.xslt")]
        private string _transform = null;

        /// <summary>
        /// _state
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + "/VEX.Battery.config.xml")]
        private VEXBatteryState _state = new VEXBatteryState();

        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/VEX/battery", AllowMultipleInstances = true)]
        private VEXBatteryOperations _mainPort = new VEXBatteryOperations();

        /// <summary>
        /// Alternate port for the generic port
        /// </summary>
        [AlternateServicePort("/battery",
            AllowMultipleInstances = true,
            AlternateContract = battery.Contract.Identifier)]
        private battery.BatteryOperations _genericPort = new battery.BatteryOperations();

        /// <summary>
        /// Partner with the VEX Robot
        /// </summary>
        [Partner("VEX", Contract = vex.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private vex.VEXOperations _vexPort = new vex.VEXOperations();
        private vex.VEXOperations _vexNotificationPort = new vex.VEXOperations();

        /// <summary>
        /// Subscription manager partner
        /// </summary>
        [Partner(Partners.SubscriptionManagerString, Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// HTTP helper
        /// </summary>
        DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public VEXBattery(DsspServiceCreationPort creationPort) :
            base(creationPort)
        {
        }

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();
            base.Start();

            #region Initialize Service State
            // Reset the controller to know about our configuration
            Port<bool> donePort = new Port<bool>();
            SpawnIterator<Port<bool>>(donePort, InitializeKnownConnection);
            Activate(Arbiter.Receive(false, donePort,
                delegate(bool success)
                {
                    if (success)
                    {
                        // display HTTP service Uri
                        LogInfo(LogGroups.Console, "Service uri: ");
                    }
                    else
                    {
                        LogError(LogGroups.Console, "VEX Analog Input service failed to start.");
                        _mainPort.Post(new DsspDefaultDrop());
                    }
                }));
            #endregion

            // Needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);
        }

        /// <summary>
        /// Initialize and Validate the state
        /// </summary>
        /// <param name="connected"></param>
        private void InitializeState()
        {
            bool changed = false;

            if (_state == null)
            {
                _state = new VEXBatteryState();
                _state.SensorPort = vex.SensorPort.SensorPort_None;
                changed = true;
            }

            // Set default Maximum Battery at 4.3 volts
            if (_state.ZenerDrop < 1.0 || _state.ZenerDrop > 7.3)
            {
                _state.ZenerDrop = 4.3;
                changed = true;
            }

            // Set default Maximum Battery at 7.3 volts
            if (_state.MaxVoltage < 6.0 || _state.MaxVoltage > 7.3)
            {
                _state.MaxVoltage = 7.3;
                changed = true;
            }

            // Set default Minimum Battery at 6 volts
            if (_state.MinVoltage < 6.0 || _state.MinVoltage > _state.MaxVoltage)
            {
                _state.MinVoltage = 6.0;
                changed = true;
            }

            if (_state.CriticalBatteryVoltage < _state.MinVoltage || _state.CriticalBatteryVoltage > _state.MaxVoltage)
            {
                _state.CriticalBatteryVoltage = 6.3;
                changed = true;
            }

            // Initialize the runtime values
            _state.TimeStamp = DateTime.MinValue;
            _state.PercentBatteryPower = 0.0;
            _state.CurrentBatteryVoltage = 0.0;

            if (changed)
            {
                SaveState(_state);
            }
        }

        /// <summary>
        /// Initialize this object based upon the current state.  Additionally,
        /// update the controller to our configuration
        /// </summary>
        /// <param name="donePort"></param>
        /// <returns></returns>
        private IEnumerator<ITask> InitializeKnownConnection(Port<bool> donePort)
        {
            bool serviceSuccess = true;

            // Configure default state
            if (_state.SensorPort == vex.SensorPort.SensorPort_None)
            {
                LogError(_state.Name + ": No sensor port selected");
                serviceSuccess = false;
            }
            else
            {
                // Configure default state
                vex.SelectivePortConfigure newConfig = new vex.SelectivePortConfigure();
                newConfig.Body.Port = _state.SensorPort;
                newConfig.Body.Type = vex.SensorDefinition.SensorType.AnalogInput;

                _vexPort.Post(newConfig);
                yield return Arbiter.Choice(newConfig.ResponsePort,
                    delegate(DefaultReplaceResponseType response)
                    {
                    },
                    delegate(Fault fault)
                    {
                        serviceSuccess = false;
                        LogError(LogGroups.Console, "Failed to update VEX service configuration", fault);
                    });

                // Subscribe to VEX for notifications
                if (serviceSuccess)
                {
                    serviceSuccess = false;
                    SaveState(_state);
                    Port<bool> subscribeDonePort = new Port<bool>();
                    SpawnIterator<Port<bool>>(subscribeDonePort, SubscribeToVEX);
                    yield return (Arbiter.Receive<bool>(false, subscribeDonePort,
                        delegate(bool success)
                        {
                            serviceSuccess = success;
                        }));
                }
            }

            donePort.Post(serviceSuccess);
            yield break;
        }

        /// <summary>
        /// Subscribe to appropriate sensor type and port on VEX
        /// Hook up notifications
        /// and then post success or failure
        /// <param name="successPort"></param>
        /// </summary>
        private IEnumerator<ITask> SubscribeToVEX(Port<bool> successPort)
        {
            // Create a custom subscription request
            vex.CustomSubscribeRequestType request = new vex.CustomSubscribeRequestType();

            // Select only the sensor and port we want
            // NOTE: this name must match the VEX sensor name.
            request.Sensors = new List<vex.SensorDefinition>();
            vex.SensorDefinition sensor = new vex.SensorDefinition();
            sensor.Type = vex.SensorDefinition.SensorType.AnalogInput;
            sensor.Port = _state.SensorPort;
            request.Sensors.Add(sensor);

            // Subscribe to the VEX and wait for a response
            yield return
                Arbiter.Choice(_vexPort.SelectiveSubscribe(request, _vexNotificationPort),
                    delegate(SubscribeResponseType Rsp)
                    {
                        base.MainPortInterleave.CombineWith(new Interleave(
                            new ExclusiveReceiverGroup(
                                Arbiter.Receive<vex.Update>(true, _vexNotificationPort, SensorNotificationHandler)
                            ),
                            new ConcurrentReceiverGroup()));

                        // Update our state with subscription status
                        successPort.Post(true);
                    },
                    delegate(Fault fault)
                    {
                        LogError("Analog Input subscription failed", fault);
                        successPort.Post(false);
                    }
                );

            yield break;
        }

        /// <summary>
        /// Receive Battery Notifications
        /// </summary>
        /// <param name="update"></param>
        private void SensorNotificationHandler(vex.Update update)
        {
            if (update != null)
            {
                double batteryLevel = update.Body.PortSensors.AnalogValues[(int)_state.SensorPort - 1];
                batteryLevel = batteryLevel * 5.0 / 1024.0; // Scale to range of measurement
                batteryLevel += _state.ZenerDrop;    // Zener diode drop.

                double percentBatteryPower = 100.0 * (batteryLevel - _state.MinVoltage) / (_state.MaxVoltage - _state.MinVoltage);
                percentBatteryPower = Math.Min(100.0, Math.Max(0.0, percentBatteryPower));
                if (_state.PercentBatteryPower != percentBatteryPower
                    || _state.CurrentBatteryVoltage != batteryLevel)
                {
                    _state.TimeStamp = DateTime.Now;
                    _state.CurrentBatteryVoltage = batteryLevel;
                    _state.PercentBatteryPower = percentBatteryPower;
                    battery.BatteryState genericState = new battery.BatteryState();
                    SendNotification<battery.Replace>(_subMgrPort, _state.ToGenericState(genericState));
                }
                if (_state.CurrentBatteryVoltage < _state.CriticalBatteryVoltage)
                {
                    battery.UpdateCriticalBattery criticalBattery = new battery.UpdateCriticalBattery();
                    criticalBattery.PercentCriticalBattery = _state.PercentBatteryPower;
                    SendNotification<battery.SetCriticalLevel>(_subMgrPort, criticalBattery);
                }
            }
        }


        #region Main Port Handlers

        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet get)
        {
            get.ResponsePort.Post(new HttpResponseType(HttpStatusCode.OK, _state, _transform));
            yield break;
        }

        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            // Use helper to read form data
            ReadFormData readForm = new ReadFormData(httpPost);
            _httpUtilities.Post(readForm);

            // Wait for result
            Activate(Arbiter.Choice(readForm.ResultPort,
                delegate(NameValueCollection parameters)
                {
                    if (parameters["SetValues"] != null)
                    {
                        // Update our state variables
                        _state.Name = parameters["Name"];

                        double temp = 0;
                        double.TryParse(parameters["ZenerDrop"], out temp);
                        _state.ZenerDrop = temp;

                        double.TryParse(parameters["MaxVoltage"], out temp);
                        _state.MaxVoltage = temp;

                        double.TryParse(parameters["CriticalBatteryVoltage"], out temp);
                        _state.CriticalBatteryVoltage = temp;

                        double.TryParse(parameters["MinVoltage"], out temp);
                        _state.MinVoltage = temp;

                        // Make sure everything is within reasonable bounds
                        InitializeState();

                        battery.BatteryState genericState = new battery.BatteryState();
                        SendNotification<battery.Replace>(_subMgrPort, _state.ToGenericState(genericState));
                    }
                    HttpPostSuccess(httpPost);
                },
                delegate(Exception fault)
                {
                    LogError(fault.Message);
                    HttpPostFailure(httpPost, fault.Message);
                })
            );
            yield break;
        }

        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        /// <param name="httpPost"></param>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _state, _transform);
            httpPost.ResponsePort.Post(rsp);
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        /// <param name="httpPost"></param>
        /// <param name="failureReason"></param>
        private static void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.ExpectationFailed, failureReason);
            httpPost.ResponsePort.Post(rsp);
        }

        #endregion

        #region Generic Port Handlers
        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> GetHandler(battery.Get get)
        {
            battery.BatteryState genericState = new battery.BatteryState();
            get.ResponsePort.Post(_state.ToGenericState(genericState));
            yield break;
        }

        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> ReplaceHandler(battery.Replace replace)
        {
            _state.CopyFrom(replace.Body);
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> SubscribeHandler(battery.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);

            battery.BatteryState genericState = new battery.BatteryState();
            SendNotificationToTarget<battery.Replace>(subscribe.Body.Subscriber, _subMgrPort, new battery.Replace(_state.ToGenericState(genericState)));
            yield break;
        }

        /// <summary>
        /// SetCriticalLevel Handler
        /// </summary>
        /// <param name="setCriticalLevel"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> SetCriticalLevelHandler(battery.SetCriticalLevel setCriticalLevel)
        {
            _state.CriticalBatteryVoltage = _state.MaxVoltage * setCriticalLevel.Body.PercentCriticalBattery;
            SendNotification<battery.SetCriticalLevel>(_subMgrPort, setCriticalLevel);
            setCriticalLevel.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        #endregion
    }


    /// <summary>
    /// Battery Contract
    /// </summary>
    public sealed class Contract
    {
        /// The Unique Contract Identifier for the Battery service
        [DataMember, Description("Identifies the unique DSS Contract Identifier for the Battery.")]
        public const string Identifier = "http://schemas.vexrobotics.com/2011/11/vexextras.battery.html";

        /// <summary>
        /// The Battery Device Type
        /// </summary>
        [DataMember, Description("Identifies the device model.")]
        public const string DeviceModel = "Battery";
    }

    /// <summary>
    /// Battery State
    /// </summary>
    [DataContract, Description("Specifies the battery\'s state.")]
    public class VEXBatteryState : IDssSerializable, ICloneable
    {
        /// <summary>
        /// Name of the battery
        /// </summary>
        [DataMember, Description("Name of the battery")]
        public string Name;

        /// <summary>
        /// Hardware ID of the analog sensor used to measure the battery
        /// </summary>
        [DataMember, Description("Identifies the sensor port measuring the battery voltage.")]
        public vex.SensorPort SensorPort;

        /// <summary>
        /// Voltage drop across the Zener diode
        /// </summary>
        [DataMember, Description("Voltage drop across the Zener diode")]
        public double ZenerDrop;

        /// <summary>
        /// Full battery power
        /// </summary>
        [DataMember, Description("Identifies the power setting at which the battery is fully charged. \n(Suggested 7.3 volts)")]
        public double MaxVoltage;

        /// <summary>
        /// Critical battery voltage
        /// </summary>
        [DataMember, Description("Indicates the battery voltage at which operation may be impaired. \n(Suggested 6.3 volts)")]
        public double CriticalBatteryVoltage;

        /// <summary>
        /// Minimum battery voltage;
        /// </summary>
        [DataMember, Description("Indicates the minimum battery voltage. \n(Suggested 6.0 volts)")]
        public double MinVoltage;

        /// <summary>
        /// Percentage of remaining battery power
        /// between 0% and 100%
        /// </summary>
        [DataMember, Description("Indicates the percentage of battery power remaining. (0% - 100%)")]
        [Browsable(false)]
        public double PercentBatteryPower;

        /// <summary>
        /// Current Battery Voltage
        /// </summary>
        [DataMember, Description("Indicates the current battery voltage.")]
        [Browsable(false)]
        public double CurrentBatteryVoltage;

        /// <summary>
        /// The time of the last battery reading
        /// </summary>
        [DataMember, Description("Indicates the time of the last battery reading.")]
        [Browsable(false)]
        public DateTime TimeStamp;

        /// <summary>
        /// Copy To the generic battery state
        /// </summary>
        /// <param name="genericBattery"></param>
        public battery.BatteryState ToGenericState(battery.BatteryState genericBattery)
        {
            genericBattery.MaxBatteryPower = MaxVoltage;
            genericBattery.PercentBatteryPower = PercentBatteryPower;
            genericBattery.PercentCriticalBattery = CriticalBatteryVoltage / MaxVoltage;
            return genericBattery;
        }

        /// <summary>
        /// Copy From the generic battery state
        /// </summary>
        /// <param name="genericBattery"></param>
        public void CopyFrom(battery.BatteryState genericBattery)
        {
            MaxVoltage = genericBattery.MaxBatteryPower;
            CriticalBatteryVoltage = MaxVoltage * genericBattery.PercentCriticalBattery;
            PercentBatteryPower = genericBattery.PercentBatteryPower;
            CurrentBatteryVoltage = MaxVoltage * PercentBatteryPower;
        }

        #region IDssSerializable

        /// <summary>
        /// Copy To Analog Input sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXBatteryState typedTarget = target as VEXBatteryState;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.CriticalBatteryVoltage = this.CriticalBatteryVoltage;
            typedTarget.CurrentBatteryVoltage = this.CurrentBatteryVoltage;
            typedTarget.MaxVoltage = this.MaxVoltage;
            typedTarget.MinVoltage = this.MinVoltage;
            typedTarget.Name = this.Name;
            typedTarget.PercentBatteryPower = this.PercentBatteryPower;
            typedTarget.SensorPort = this.SensorPort;
            typedTarget.TimeStamp = this.TimeStamp;
            typedTarget.ZenerDrop = this.ZenerDrop;
        }
        /// <summary>
        /// Clone Analog Input sensor state
        /// </summary>
        public virtual object Clone()
        {
            VEXBatteryState target = new VEXBatteryState();

            target.CriticalBatteryVoltage = this.CriticalBatteryVoltage;
            target.CurrentBatteryVoltage = this.CurrentBatteryVoltage;
            target.MaxVoltage = this.MaxVoltage;
            target.MinVoltage = this.MinVoltage;
            target.Name = this.Name;
            target.PercentBatteryPower = this.PercentBatteryPower;
            target.SensorPort = this.SensorPort;
            target.TimeStamp = this.TimeStamp;
            target.ZenerDrop = this.ZenerDrop;

            return target;

        }
        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(CriticalBatteryVoltage);
            writer.Write(CurrentBatteryVoltage);
            writer.Write(MaxVoltage);
            writer.Write(MinVoltage);
            writer.Write(Name);
            writer.Write(PercentBatteryPower);
            writer.Write((int)SensorPort);
            writer.Write(ZenerDrop);
            Microsoft.Dss.Services.Serializer.BinarySerializationHelper.SerializeDateTime(TimeStamp, writer);
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            CriticalBatteryVoltage = reader.ReadDouble();
            CurrentBatteryVoltage = reader.ReadDouble();
            MaxVoltage = reader.ReadDouble();
            MinVoltage = reader.ReadDouble();
            Name = reader.ReadString();
            PercentBatteryPower = reader.ReadDouble();
            SensorPort = (vex.SensorPort)reader.ReadInt32();
            ZenerDrop = reader.ReadDouble();
            TimeStamp = Microsoft.Dss.Services.Serializer.BinarySerializationHelper.DeserializeDateTime(reader);

            return this;
        }

        #endregion
    }
}


